/*
 * Sight.cpp
 *
 *  Created on: Feb 15, 2010
 *      Author: vuk
 */

#include "Sight.h"
#include <string>

Sight::Sight():
	ESC(0),
//	wide(42),
	wide(42),
	//taster(0),
	color(NULL),
	b("blue"), g("green"), y("yellow"), r("red"), w("white"), c("crna"),
	cap(0),
//	rawframe(Size(160,120),CV_8SC3),
	frame(Size(160,120),CV_8SC3),
	warp_matrix(3,3,CV_32FC1)
{
//	cap>>rawframe;
//	resize(rawframe, frame, Size(160,120), 2, 2, INTER_AREA);
//	resize(frame, rawframe, Size(160,120), 2, 2, INTER_AREA);
	namedWindow("cam1raw", CV_WINDOW_AUTOSIZE);
	namedWindow("cam1", CV_WINDOW_AUTOSIZE);

//Setuje sve tacke za promenu perspektive
	frameQuad[0].x = 0;                 //frame Top left
	frameQuad[0].y = 0;
	frameQuad[1].x = frame.cols;   //frame Top right
	frameQuad[1].y = 0;
	frameQuad[2].x = 0;                 //frame Bottom left
	frameQuad[2].y = frame.rows;
	frameQuad[3].x = frame.cols;   //frame Bottom right
	frameQuad[3].y = frame.rows;
	positionQuad[0].x  = -25;//position  Top left
	positionQuad[0].y  = 0;
	positionQuad[1].x  = frame.cols+25; //position   Top right
	positionQuad[1].y  = 0;
	positionQuad[2].x  = 34;//position   Bottom left
	positionQuad[2].y  = frame.rows;
	positionQuad[3].x  = frame.cols-34; //position   Botttom right
	positionQuad[3].y  = frame.rows;

	warp_matrix=getPerspectiveTransform(frameQuad, positionQuad);
}

Sight::~Sight()
{
	cvDestroyWindow("cam1raw");
	cvDestroyWindow("cam1");
}

void Sight::refresh()
{
	cap>>rawframe;
}

void Sight::showFrame()
{
	Mat pom;
	cvtColor(frame, pom, CV_HSV2BGR);
	imshow("cam1",pom);
}

void Sight::showMiniRawFrame()
{
	Mat pom(Size(160,120),CV_8SC3);
	resize(rawframe, pom, Size(160,120), 1, 1, INTER_NEAREST);
	//cvtColor(pom, pom, CV_HSV2BGR);
	imshow("cam1raw",pom);
}

void Sight::showRawFrame()
{
	imshow("cam1raw",rawframe);
}


void Sight::makeFilteredFrame()
{
	//rectangle(frame, Point(0,0), Point(frame.cols, frame.rows), Scalar (0,128,0), -1);
//	imshow("cam1raw",rawframe);
	resize(rawframe, frame, Size(160,120), 2, 2, INTER_NEAREST);
	cvtColor(frame, rawframe, CV_BGR2HSV);
	cvtColor(frame, frame, CV_BGR2HSV);

	for(int i=0; i<frame.rows; i++)
		for(unsigned int j=0; j<frame.step ; j+=frame.channels())
		{

			if(g==frame.data+(i*frame.step)+j)
				g.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else if(b==frame.data+(i*frame.step)+j)
				b.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else if(y==frame.data+(i*frame.step)+j)
				y.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else if(r==frame.data+(i*frame.step)+j)
				r.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else if(w==frame.data+(i*frame.step)+j)
				w.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else if(c==frame.data+(i*frame.step)+j)
				c.fillwidthrepresent(rawframe.data+(i*frame.step)+j);
			else  *(rawframe.data+(i*frame.step)+j+2)=0;
		}
	warpPerspective(rawframe, frame, warp_matrix,frame.size(), INTER_NEAREST);
}

void Sight::changeColors(const char &taster)
{
	switch(taster)
	{
		case('`'): cvDestroyWindow ((color->getName()+" boja").c_str());color=NULL ; break;
		case('1'):if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&c; namedWindow(c.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('2'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&g; namedWindow(g.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('3'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&y; namedWindow(y.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('4'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&r; namedWindow(r.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('5'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&w; namedWindow(w.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('6'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
								 color=&b; namedWindow(b.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
	}
	if(color)
	{
		color->config(taster);
		Mat colorframe;
		resize(rawframe, colorframe, Size(160,120), 2, 2, INTER_NEAREST);
		cvtColor(colorframe, colorframe, CV_BGR2HSV);
		for(int i=0; i<colorframe.rows; i++)
			for(unsigned int j=0; j<colorframe.step ; j+=colorframe.channels())
			{
				if(*color==colorframe.data+(i*colorframe.step)+j)
					color->fillwidthrepresent(colorframe.data+(i*colorframe.step)+j);
				else
					*(colorframe.data+(i*colorframe.step)+j+2)=0;
			}
		cvtColor(colorframe, colorframe, CV_HSV2BGR);
		imshow((color->getName()+" boja").c_str(),colorframe);
	}
}

void Sight::findRed(int &X, int &Y, int fspace, int erspace)
{
	r.findobject(X,Y,fspace,erspace,frame);
}

void Sight::roadObsticle(int &X, int &Y, int fspace, int erspace)
{
	uchar *tr=NULL;
	uchar *tl=NULL;
	int AX, AY;
	AX=AY=0;
	int errorcount;
	for(int i=frame.rows; i>fspace; i--)
	{
		for(int j=0; j < frame.cols/2-fspace/2-wide; j++)
		{
			errorcount=0;
			tr= frame.data+( i*frame.step + (frame.cols/2+j)*frame.channels() );
			if(g.getRepresent().H()!=*tr)
			{
				for(int up=0; up<fspace; up++)
				{
					for(int s=0; s<fspace/2; s++)
					{

						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j+s) * frame.channels() );
						//if(c.r(tr) || c.g(tr) || c.b(tr))
						if(g.getRepresent().H()==*tr)
							errorcount++;

						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j-s) * frame.channels());
						//if(c.r(tr) || c.g(tr) || c.b(tr))
						if(g.getRepresent().H()==*tr)
							errorcount++;

						if(errorcount>=erspace)
							break;
					}
					if(errorcount>=erspace)
						break;

				}
				if(errorcount<erspace)
				{
					AX=j;
					AY=frame.rows-i;
					break;
				}
			}

			if(AY)
				break;

			tl= frame.data+(i*frame.step - (frame.cols/2+j)*frame.channels() );

//			//if(!c.r(tl) && !c.g(tl) && !c.b(tl))
			if(g.getRepresent().H()!=*tl)
			{

				for(int up=0; up<fspace; up++)
				{
					for(int s=0; s<fspace/2; s++)
					{

						tl= frame.data+( (i-up)*frame.step - (frame.cols/2+j+s) * frame.channels() );

						//if(c.r(tl) || c.g(tl) || c.b(tl))
						if(g.getRepresent().H()==*tl)
							errorcount++;

						tl= frame.data+((i-up)*frame.step - (frame.cols/2+j-s) * frame.channels());

						//if(c.r(tl) || c.g(tl) || c.b(tl))
						if(g.getRepresent().H()==*tl)
							errorcount++;

						if(errorcount>=erspace)
							break;
					}
					if(errorcount>=erspace)
						break;
				}

				if(errorcount<erspace)
				{
					AX=-j;
					AY=frame.rows-i;
					break;
				}
			}

		}
		if(AY)
			break;
	}
	X=AX;
	Y=AY;
	//cout<<"X= "<<X<<" Y= "<<Y<<endl;
}

//void Sight::findCorn(int &X, int &Y, int fspace, int erspace)
//{
//	uchar *tr=NULL;
//	uchar *tl=NULL;
//	int AX, AY;
//	AX=AY=0;
//	int errorcount;
//	for(int i=frame.rows; i>fspace; i--)
//	{
//		for(int j=0; j < frame.cols/2-fspace/2-wide; j++)
//		{
//			errorcount=0;
//			tr= frame.data+( i*frame.step + (frame.cols/2+j)*frame.channels() );
//			if(w.getRepresent().H()==*tr)
//			{
//				for(int up=0; up<fspace; up++)
//				{
//					for(int s=0; s<fspace/2; s++)
//					{
//
//						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j+s) * frame.channels() );
//						//if(w.r(tr) || w.g(tr) || w.b(tr))
//						if(w.getRepresent().H()!=*tr)
//							errorcount++;
//
//						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j-s) * frame.channels());
//						//if(w.r(tr) || w.g(tr) || w.b(tr))
//						if(w.getRepresent().H()!=*tr)
//							errorcount++;
//
//						if(errorcount>=erspace)
//							break;
//					}
//					if(errorcount>=erspace)
//						break;
//
//				}
//				if(errorcount<erspace)
//				{
//					AX=j;
//					AY=frame.rows-i;
//					break;
//				}
//			}
//
//			if(AY)
//				break;
//
//			tl= frame.data+(i*frame.step - (frame.cols/2+j)*frame.channels() );
//
////			//if(!w.r(tl) && !w.g(tl) && !w.b(tl))
//			if(w.getRepresent().H()==*tl)
//			{
//
//				for(int up=0; up<fspace; up++)
//				{
//					for(int s=0; s<fspace/2; s++)
//					{
//
//						tl= frame.data+( (i-up)*frame.step - (frame.cols/2+j+s) * frame.channels() );
//
//						//if(w.r(tl) || w.g(tl) || w.b(tl))
//						if(w.getRepresent().H()!=*tl)
//							errorcount++;
//
//						tl= frame.data+((i-up)*frame.step - (frame.cols/2+j-s) * frame.channels());
//
//						//if(w.r(tl) || w.g(tl) || w.b(tl))
//						if(w.getRepresent().H()!=*tl)
//							errorcount++;
//
//						if(errorcount>=erspace)
//							break;
//					}
//					if(errorcount>=erspace)
//						break;
//				}
//
//				if(errorcount<erspace)
//				{
//					AX=-j;
//					AY=frame.rows-i;
//					break;
//				}
//			}
//
//		}
//		if(AY)
//			break;
//	}
//	X=AX;
//	Y=AY;
//	//cout<<"X= "<<X<<" Y= "<<Y<<endl;
//}

void Sight::roadObstacle(int &X, int &Y, int fspace, int erspace)
{
	uchar *tr=NULL;
	uchar *tl=NULL;
	int AX, AY;
	AX=AY=0;
	int errorcount;
	for(int i=frame.rows; i>fspace; i--)
	{
		for(int j=0; j < frame.cols/2-fspace/2-wide; j++)
		{
			errorcount=0;
			tr= frame.data+( i*frame.step + (frame.cols/2+j)*frame.channels() );
			if(c.getRepresent().H()==*tr)
			{
				for(int up=0; up<fspace; up++)
				{
					for(int s=0; s<fspace/2; s++)
					{

						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j+s) * frame.channels() );
						//if(c.r(tr) || c.g(tr) || c.b(tr))
						if(c.getRepresent().H()!=*tr)
							errorcount++;

						tr= frame.data+( (i-up)*frame.step + (frame.cols/2+j-s) * frame.channels());
						//if(c.r(tr) || c.g(tr) || c.b(tr))
						if(c.getRepresent().H()!=*tr)
							errorcount++;

						if(errorcount>=erspace)
							break;
					}
					if(errorcount>=erspace)
						break;

				}
				if(errorcount<erspace)
				{
					AX=j;
					AY=frame.rows-i;
					break;
				}
			}

			if(AY)
				break;

			tl= frame.data+(i*frame.step - (frame.cols/2+j)*frame.channels() );

//			//if(!c.r(tl) && !c.g(tl) && !c.b(tl))
			if(c.getRepresent().H()==*tl)
			{

				for(int up=0; up<fspace; up++)
				{
					for(int s=0; s<fspace/2; s++)
					{

						tl= frame.data+( (i-up)*frame.step - (frame.cols/2+j+s) * frame.channels() );

						//if(c.r(tl) || c.g(tl) || c.b(tl))
						if(c.getRepresent().H()!=*tl)
							errorcount++;

						tl= frame.data+((i-up)*frame.step - (frame.cols/2+j-s) * frame.channels());

						//if(c.r(tl) || c.g(tl) || c.b(tl))
						if(c.getRepresent().H()!=*tl)
							errorcount++;

						if(errorcount>=erspace)
							break;
					}
					if(errorcount>=erspace)
						break;
				}

				if(errorcount<erspace)
				{
					AX=-j;
					AY=frame.rows-i;
					break;
				}
			}

		}
		if(AY)
			break;
	}
	X=AX;
	Y=AY;
	//cout<<"X= "<<X<<" Y= "<<Y<<endl;
}




